//--------------------
// SPI_Wakeup_Master.c
//--------------------

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "LPC8xx.h"
#include "spi.h"
#include "syscon.h"
#include "swm.h"
#include "utilities.h"
#include "gpio.h"
/////////////算法参数定义////////
//控制器参数
double eita = 0.5;
double miu = 0.05;
double rou= 0.5;
double lamda = 0.05;
double epsilon = 0.00001;

//控制器伪偏导数初值
double fai1=0.3,fai2=0;
double error2 = 0;

//double td = 4000.072540;//期望值
//double td = 4000.183598;//期望值
//double td =4000.072598;      ---->40.001 081
//double td = 4000.183098;//期望值  --->39.999 979 288
 // double td = 4000.183120; //期望值  --->39.999 972 227  -28
 //double td = 4000.162120; //期望值  --->  39.999 984 234 -16
 // double td = 4000.142120; //期望值  --->  40.000 367 192 +367
 //double td =  4000.152120;  //期望值  --->  40.000 270 192 +270
 //  double td =  4000.160120;   //期望值  --->  40.000 181 192 +181
 // double td =  4000.162085;   //期望值  --->  40.000 210 192 +210
   double td =  4000.182015;     //期望值  --->    40.000 006 852 +6
//double t1=td;
   // 10 - 15
   /*
    *  double td =  4000.183015;     //期望值  --->    39.999 850 852 -150
    *	double td =  4000.163015;    //期望值  --->    40.000 053 052 +50
    *   double td =  4000.168015;     //期望值  --->   39.999 990 852 -10
    *
    *
    */

   // 10 - 19
   /*
    *  double td =  4000.168015;     //期望值  --->    40.000 155 852 +155

    *
    *
    */

double yd = 0; //PPB
double y1=0,y2=0,y3=0;

double dac_data1=524287,dac_data2=0;//类型可能要改 unsigned int
double tdc_data=0;
double du1=0,du2= 0;//电压数据差
double sum=0;
/////////////////////////////////
#define SPIBAUD  1000000
#define SCK_PIN  P0_24
#define MOSI_PIN P0_25
#define SSEL_PIN P0_21
#define CLK_H    GPIOSetBitValue(  0, 20,  1 )
#define CLK_L    GPIOSetBitValue(  0, 20,  0 )
#define SDA_H    GPIOSetBitValue(  0, 21,  1 )
#define SDA_L    GPIOSetBitValue(  0, 21,  0 )
#define CS_H     GPIOSetBitValue(  0, 22,  1 )
#define CS_L     GPIOSetBitValue(  0, 22,  0 )

const char config_register[17] = {0x83,0x03,0x00,0x40,0x0D,0x03,0xC0,0xA3,0xA1,0x13,0x00,0x0A,0xCC,0x05,0xF1,0x7D,0x04};
extern void setup_debug_uart(void);
unsigned char MySPIByte(unsigned char sdat,unsigned char cs,unsigned char rxignore);
void gp22_wr_config_reg ( uint8_t opcode_address,  uint32_t config_reg_data);
float gp22_read_n_bytes(uint8_t n_bytes, uint8_t read_opcode, uint8_t read_addr, uint8_t fractional_bits);
void WriteOneByte(unsigned char command);
double tdc6500();
////数据驱动API////
int sign(double x);
double round(double r);
void data_driver();

void delay1ms(void)
{
    unsigned char a,b;
        for(b=86;b>0;b--)
            for(a=13;a>0;a--);
}
void delay(int xx)
{
  int a,b;
 for(a=0;a<50;a++)
 for(b=xx;b>0;b--);
}
typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
void dac1220_init(void)
{
	////////////////////SCLK reset pattern
	CS_L; //RST = 0
	//delay_ms(30);
	CLK_L;  //SCLK = 0
	delay(500);
	CLK_H;  //SCLK = 1
	delay(500);
	CLK_L;  //SCLK = 0
	delay(10);
	CLK_H;  //SCLK = 1
	delay(1200);
	CLK_L;  //SCLK = 0
	delay(20);
	delay(2000);
	CLK_L;  //SCLK = 0
	CS_H;
	delay_ms(5);//等待重启
	CS_L;
	WriteOneByte(0x04);
	WriteOneByte(0x22);
	CS_H;
	delay1ms;
	CS_L;
	WriteOneByte(0x05);
	WriteOneByte(0xe1);
	CS_H;
	delay1ms;
}
void dac1220_set(unsigned char data1,unsigned char data2,unsigned char data3)
{
	CS_L;
	WriteOneByte(0x40);
	WriteOneByte(data1);
	WriteOneByte(data2);
	WriteOneByte(data3);
	delay1ms;//no must
	CS_H;
}

#define BUFFER_LIMIT 65
int irq_flag;
unsigned char rx_buffer[10];
uint32_t rx_char_counter = 0;
volatile enum {false, true} handshake;
void UART0_IRQHandler()
{
	unsigned char temp;
	temp = LPC_USART0->RXDAT ; //接收器数据寄存器。 包含收到的最后一个字符。
	rx_buffer[rx_char_counter] = temp;        // Append the current character to the rx_buffer
  if (temp == 0x0A) {                       // CR (carriage return) is current character. End of string.
 // rx_buffer[rx_char_counter+1] = 0x0A;    // Append a new line character to rx_buffer.
 // rx_buffer[rx_char_counter+2] = 0x00;    // Append a NUL terminator character to rx_buffer to complete the string.
 // WaitForUART0txRdy;                      // Wait for TXREADY
 // LPC_USART0->TXDAT  = 0x0A;              // Echo a NL (new line) character to the terminal.
    handshake = true;                       // Set handshake for main()
    rx_char_counter = 0;                    // Clear array index counter
    irq_flag = 1 ;
	}
  else {                                  // Current character is not CR, keep collecting them.
    rx_char_counter++;                      // Increment array index counter.
    if (rx_char_counter == BUFFER_LIMIT)  // If the string overruns the buffer, stop here before all hell breaks lose.
    	rx_char_counter =0;
	}
	return;
}
int main(void) {
	setup_debug_uart();
	// Enable clocks to relevant peripherals
	LPC_SYSCON->SYSAHBCLKCTRL[0] |= (SPI0|SWM|IOCON|GPIO1);
	LPC_SYSCON->SYSAHBCLKCTRL[0] |= GPIO0;   // Turn on clock to GPIO0
	// GPIOInit();  //GPIO初始化  有问题
	LPC_SYSCON->PRESETCTRL[0] &= (~(1<<6) & ~(1<<20));
	LPC_SYSCON->PRESETCTRL[0] |= ~(~(1<<6) & ~(1<<20));
	GPIOSetDir( 0, 20, 1 );
	GPIOSetDir( 0, 21, 1 );
	GPIOSetDir( 0, 22, 1 );
	CLK_L;
	CS_L;
	dac1220_init();
	// Configure the SWM (see peripherals_lib and swm.h)
	ConfigSWM(SPI0_SCK, 6);
	ConfigSWM(SPI0_MOSI, 7);
	ConfigSWM(17, 50);
	ConfigSWM(SPI0_SSEL0, 39);

	// Give SPI0 a reset
	LPC_SYSCON->PRESETCTRL[0] &= (SPI0_RST_N);
	LPC_SYSCON->PRESETCTRL[0] |= ~(SPI0_RST_N);
	// Enable main_clk as function clock to SPI
	LPC_SYSCON->SPI0CLKSEL = FCLKSEL_MAIN_CLK;
	// Get main_clk frequency
	SystemCoreClockUpdate();
	// Configure the SPI master's clock divider (value written to DIV divides by value+1)
	LPC_SPI0->DIV = (main_clk/SPIBAUD) - 1;
	// Configure the CFG register:
	// Enable=true, master, no LSB first, CPHA=0, CPOL=0, no loop-back, SSEL active low
	LPC_SPI0->CFG = CFG_ENABLE | CFG_MASTER | CFG_CPHA;//CPHA=1, CPOL=0？？？？
	// Configure the SPI delay register (DLY)
	// Pre-delay = 0 clocks, post-delay = 0 clocks, frame-delay = 0 clocks, transfer-delay = 0 clocks
	LPC_SPI0->DLY = 0x0000;
	// Configure the SPI control register
	// Master: End-of-frame true, End-of-transfer true, RXIGNORE true, LEN 8 bits.
	LPC_SPI0->TXCTL = CTL_EOF | CTL_EOT | CTL_LEN(8);//1<<16可能有问题
	GPIOSetDir(0,0,0);//配置中断引脚为输入GPIOSetDir( uint32_t portNum, uint32_t bitPosi, uint32_t dir )
	GPIOGetPinValue(0, 0);

	GPIOSetDir( 1, 17, 1 );//rst_id
	GPIOSetBitValue( 1, 17, 0);

	GPIOSetDir( 1, 0, 1 );//FPGA
	GPIOSetBitValue( 1, 0, 0);

	while(1)
	{
		//if(irq_flag){
		  //dac1220_set(0xFF,0x00,0x00);
		  //tdc6500();
		  data_driver();
		//}
	}// end of while(1)

}  // end of main

unsigned char MySPIByte(unsigned char sdat,unsigned char cs,unsigned char rxignore) //cs=0 or 1, rxignore=0 or 1,1 means ignore recived data
{       //sdat means the sending data
	unsigned char rdat=0;
	if(cs)  //CS=1 after send dat
	{
		LPC_SPI0->TXCTL |= (1<<20); //EOT=1
	}
	else   //CS=0 after send dat
	{
		LPC_SPI0->TXCTL &= ~(1<<20); //EOT=0
	}
	if(rxignore)
	{
		LPC_SPI0->TXCTL |=(1<<22); //Ignore
	}
	else
	{
		LPC_SPI0->TXCTL &= ~(1<<22); //Read data
	}
	while((LPC_SPI0->STAT & (1<<1))==0); //Wait Transmit Rdy
	LPC_SPI0->TXDAT=sdat;
	if(!rxignore)
	{
		while((LPC_SPI0->STAT & (1<<0))==0); //Wait Recv Rdy
		rdat=LPC_SPI0->RXDAT;
	}
	return rdat;
}
void gp22_wr_config_reg ( uint8_t opcode_address,  uint32_t config_reg_data)
{
	uint8_t Data_Byte_Lo    = config_reg_data;
	uint8_t Data_Byte_Mid1  = config_reg_data>>8;
	uint8_t Data_Byte_Mid2  = config_reg_data>>16;
	uint8_t Data_Byte_Hi    = config_reg_data>>24;

	MySPIByte(opcode_address,0,1);
	MySPIByte(Data_Byte_Hi,0,1);
	MySPIByte(Data_Byte_Mid2,0,1);
	MySPIByte(Data_Byte_Mid1,0,1);
	MySPIByte(Data_Byte_Lo,1,1);

}
float gp22_read_n_bytes(uint8_t n_bytes, uint8_t read_opcode, uint8_t read_addr, uint8_t fractional_bits)
{
	uint32_t    Result_read = 0,dat;
	float       Result = 0;
	uint8_t     read_opcode_addr = read_opcode | read_addr;
  //.............. Result = n Byte = n x 8 bits......................
	MySPIByte(read_opcode_addr,0,1); // READ OPCODE + Address
	for (int n = 0; n < n_bytes-1; n++)
    {
		dat=MySPIByte(0x00,0,0);
		Result_read=Result_read | (dat & 0xFF);
		Result_read<<=8;
    }
	dat=MySPIByte(0x00,1,0);
   	Result_read=Result_read | (dat & 0xFF);

   	Result = Result_read / pow(2, fractional_bits);
	return Result;
}
void WriteOneByte(unsigned char command)
{
	unsigned char i;
	CLK_L;
	delay(5);
	for(i = 0; i < 8;i++)
	{
		CLK_H;
		delay(5);
		if(command&0x80)
			SDA_H;
		else
			SDA_L;
		command <<= 1;
		CLK_L;
		delay(5);
	}
}

//tdc_configure
double tdc6500()
{
	unsigned char buffer=0;
	uint32_t index[2]={0,0};
	uint32_t  result[2]={0,0};
	double T=0;
	// dac1220_set(rx_buffer[0],rx_buffer[1],rx_buffer[2]);
	MySPIByte(0x30,1,1);//Power_On_Reset
	delay1ms();
	// Writing to the configuration registers (CR)
	MySPIByte(0x80,0,1);//Write Configuration
	for (int i = 0; i < 16; i++) // Send all 17 config registers via SPI
	{MySPIByte( config_register[i],0,1);}
	MySPIByte( config_register[16],1,1);
	delay1ms();//
	MySPIByte(0x40,0,1);//read Configuration
	for (int i = 0; i < 16; i++) // read all 17 config registers via SPI
	{
		MySPIByte(0x00,0,0);
	}
	MySPIByte(0x00,1,0);
	delay1ms();
	///////////////////////rst_id
	GPIOSetBitValue( 1, 17, 1);
	delay1ms();
	delay1ms();
	GPIOSetBitValue( 1, 17, 0);

	MySPIByte(0x18,1,1);//Init
	delay1ms();//no must
	delay1ms();//no must
	GPIOSetBitValue( 1, 0, 1);
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	while(GPIOGetPinValue(0, 0));
	MySPIByte(0x60+8,0,1);
	delay1ms();//no must
	delay1ms();//no must
	for(int i=0;i<100;i++)
	{delay1ms();}//调整
	for(int i=0;i<2;i++)
	{
	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+(buffer<<16);
	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+(buffer<<8);
	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+buffer;

	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+(buffer<<16);
	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+(buffer<<8);
	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+buffer;

	}
	// T=((float)result[1]-(float)result[0])/1.0e6+((float)index[1]-(float)index[0])*0.200000;
	//T=((float)result[1]-(float)result[0])/1.0e6+((float)index[1]-(float)index[0])/5;
	T=((double)result[1]-(double)result[0])/1.0e6+((double)index[1]-(double)index[0])/5;//修改了类型
	printf("\r current frequency : %10.6f\n\r", T);
	irq_flag=0;
    GPIOSetBitValue( 1, 0, 0);
    return T;
}
int sign(double x)
{
	if(x>0)
     return 1;
    else if(x==0)
     return 0;
    else
     return -1;
}
double round(double r)
{
	return (r > 0.0) ? floor(r + 0.5) : ceil(r - 0.5);
}

void data_driver()
{
	unsigned char buf_hex[3]={0};
	fai2=fai1+eita*du1*(y2-y1-fai1*du1)/(miu+du1*du1);
	//fai0=fai1;
	if ((abs(fai2)<epsilon) || (du1<(epsilon/10000)) || (sign(fai2) != sign(0.3)))
        {
		  fai2=sign(0.3)*0.01;
		}
		dac_data2 = dac_data1+rou*fai2*(yd-y2)/(lamda+fai2*fai2);
    	dac_data2 = round(dac_data2); //大于x 的最小整数
    	if ( dac_data2>16777215 )
          dac_data2 = 16777215;
        if ( dac_data2<0 )
           dac_data2 = 0;

        printf("\rdac_data2: %10.6f\n\r",dac_data2);
		for(int i=0;i<10;i++)
		{
		//DAC传值
       	buf_hex[0]=(unsigned int)(dac_data2*16)&0xff;
       	buf_hex[1]=((unsigned int)(dac_data2*16)>>8)&0xff;
       	buf_hex[2]=((unsigned int)(dac_data2*16)>>16)&0xff;
       	dac1220_set(buf_hex[2],buf_hex[1],buf_hex[0]);
       	//读取TDC数据
		tdc_data=tdc6500();
		sum+=tdc_data;
		}
		sum=sum/10;
		error2=sum-td;
		y3=-error2/(td-error2)*1e10;
		du2=dac_data2-dac_data1;

		//迭代赋值
		 fai1=fai2;
		 du1=du2;
		 y1=y2;
		 y2=y3;
		 dac_data1=dac_data2;
		 sum=0;
}
